/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "COccuMCL.h"
#include "glutImageGUI.h"   ///< For visualisation

/// For reading the data from file
int readScanLine181(FILE *f,mcl::scan &meas, mcl::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

void scan2absoluteXY( mcl::scan &s, float *x, float *y, mcl::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

float computeMSE(mcl::scan &s1,mcl::scan &s2,mcl::pose &p1,mcl::pose &p2, 
							float &x_err ,float &y_err)
{
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));

	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);

	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		if(s1.r[i] <= 0 || s2.r[i]<=0) continue;
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<1.0){
			dist += d; 
			dx2 += ddx;
			dy2 += ddy;
			num++;
		}
	}	

	ownTime_GetTimeOfDay(&ss2,&us2);
	sec = ss2-ss1;us = us2-us1;ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	//fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	//fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	//fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	//x_err = sqrt(dx2/num);
	//y_err = sqrt(dy2/num);
	if(num!=0){
		x_err = sqrt(dx2/num);
		y_err = sqrt(dy2/num);
	}
	else{
		x_err = -666;
		y_err = -666;
	}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
	
	if(num!=0){ 
		return dist/num;
	}
	return -666; 
}

void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	
  for(int j=0;j<Ny;j++){
	 for(int i=0;i<Nx;i++){
		 setRGBpix(rgb,i,j, Nx, r*data[Nx*j+i],g*data[Nx*j+i],b*data[Nx*j+i]);
	 }
	}	
}

int main(void){
	  COccuMCL omcl;
	
		//TOccupancyGrid O(40.1,40.1,0.05,Grid::TCoord2D(0,0));
		//TOccupancyGrid D(40.1,40.1,0.05,Grid::TCoord2D(0,0));
		//TOccupancyGrid G(40.1,40.1,0.05,Grid::TCoord2D(0,0));
		grid_t empty=0.0;
		grid_t rigid=254;
		grid_t occu=128;
		grid_t unknown=64;
		int Nx,Ny;
		unsigned char *rgb;
		FILE *log = fopen("j2b2_measurement.txt","rt");
		
		
		fprintf(stderr,"Initializing the map and MCL\n");
		omcl.initialize("labra.map",
							 4000, 
							 mcl::pose(2,-3,-10*M_PI/180), //mcl::pose(2,-3,-20*M_PI/180), 
							 mcl::pose(0.2,0.2,M_PI/8),
							 15,
							-M_PI/2.0,
							 0.8);
		fprintf(stderr,"Done!\n");
		omcl.O->getDim(Nx,Ny);
		
		/*
		///Scanmatch stuff
		CScanMatchInterface *matcher;  ///< This is for differential 
		CScanMatchInterface *gmatcher; ///< This is for global
		
		matcher = new CAHistogramECorrelationScanMatch(0.2, ///< Search area in meters 
																										0.4);///< Search angle in radians
		
		gmatcher = new CAHistogramECorrelationScanMatch(
																						.4,   // Search area in meters 
																						0.6);     // Search angle in radians
		*/
		
		mcl::pose u_pos, pos,gpos,odo_cur,odo_ref,est,rpos;   ///< The estimated position
		//ScanMatch::scan ref,smref;   ///< The Reference scan 
		mcl::scan meas,ref,virt;  ///< The Reference scan 
		
		meas.allocate(181);
		ref.allocate(181);
		virt.allocate(180);
		readScanLine181(log,ref,odo_cur);
		odo_cur.set(0,0,0);
		//matcher->setReferenceScan(ref,odo_cur); 
		
		///Initialize grid as empty
		//O.setGridToValue(unknown);
 		//O.setGridToValue(0.5);
		//D.setGridToValue(0.5);
		//G.setGridToValue(0.5);
		
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		///Initialize Image display
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		omcl.O->fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		//usleep(1000*1000);
		
		
		Grid::TScan s; //virt; ///< for occupancy grid
		s.alloc(181);
		//virt.alloc(181);
		readScanLine181(log,meas,odo_cur);
		
		for(int i=0;i<meas.N;i++){
				s.a[i] = meas.a[i];
				s.r[i] = meas.r[i];
		}
		odo_cur.set(0,0,0);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		//matcher->getCurrentPose(pos); ///< Get the estimate
		int cnt=0;
		odo_ref = odo_cur;
		while(!feof(log)){
				readScanLine181(log,meas,odo_cur);
				
				ownTime_GetTimeOfDay(&ss1,&us1);
				
				est.setToDifferentialPose(odo_cur,odo_ref);
				
				odo_ref = odo_cur;
				//pos = pos.integrateDifferential(est);
				
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
	      fprintf(stderr,"1: (%.4f) ",fms);
				ownTime_GetTimeOfDay(&ss1,&us1);
				//O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, occu, virt); ///<getscan
				
				//omcl.runMCL(est, mcl::pose(fabs(est.x)*0.1+0.01,fabs(est.y)*0.1+0.01,0.1*fabs(est.a)+M_PI/360),  meas);
				omcl.runMCL(est, mcl::pose(fabs(est.x)*0.03,fabs(est.y)*0.03
																						,0.001*fabs(est.a)),  meas);
				/*
				O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, (grid_t) 0.8, virt); ///<getscan
				for(int i=0;i<meas.N;i++){
						ref.a[i]=virt.a[i]-M_PI/2.0;
						ref.r[i]=virt.r[i]; 
						if (virt.r[i]<0)virt.r[i]=0;
				}
				*/
				
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"2:Get (%.4f) ",fms);
				
				//fprintf(stderr,"2:(%.2f,%.2f,%.2f) ",pos.x,pos.y,pos.a);
				ownTime_GetTimeOfDay(&ss1,&us1);
				/*
				gmatcher->setCurrentPose(pos);
				gmatcher->setReferenceScan(ref,pos); 
				gmatcher->update(meas,pos);    ///update
				gmatcher->getCurrentPose(pos); ///< Get the estimate
				*/
				//fprintf(stderr,"3:(%.2f,%.2f,%.2f) \n",pos.x,pos.y,pos.a);
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;us = us2-us1; ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				
				
				
				for(int i=0;i<meas.N;i++){
						s.a[i] = meas.a[i];
						s.r[i] = meas.r[i];
						if(s.r[i] > 35) s.r[i] = 0;
				}
				ownTime_GetTimeOfDay(&ss1,&us1);
				//if( sqrt((pos.x-u_pos.x)*(pos.x-u_pos.x) + (pos.x-u_pos.y)*(pos.y-u_pos.y))>2.0 ||
				//				fabs(pos.a-u_pos.a)>M_PI/4.0			){
				//O.setScanFillSimple(pos.x, pos.y ,pos.a, s,empty,occu,unknown);
								
				u_pos = pos;
				//}
				
				//O.setScanFillSimple(pos.x+5, pos.y+5 ,0, virt,empty,occu);
				//D.fillRGB(rgb,1.0/255.0);
				//displayImage(0, rgb,Ny,Nx);
				//usleep(1000*500);
				
				setRGBmap(rgb,omcl.O, 0,255, 0);
				Grid::TCoord2D coord;
				Grid::TPixel pix;
				for(int i=0;i<omcl.filt.NumOfParticles;i++){
					coord.x = omcl.filt.Particles[i].x;
					coord.y = omcl.filt.Particles[i].y;
					pix = omcl.O->toPixel(coord);
					setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
				}
				
				
				mcl::pose mp = omcl.filt.getDistributionMean();
				//mp.x = 3;
				//mp.y = -3;
				//mp.a=M_PI/4;
				omcl.getScan(virt, mp);
				for(int i=0;i<virt.N;i++){
					coord.x = mp.x + virt.r[i]*cos(virt.a[i]+mp.a);
					coord.y = mp.y + virt.r[i]*sin(virt.a[i]+mp.a);
					pix = omcl.O->toPixel(coord);
					if(pix.x>=0 && pix.x<Nx&&pix.y>=0&&pix.y<Ny)
					 setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0);
				}
				
				for(int i=0;i<meas.N;i++){
					coord.x = mp.x + meas.r[i]*cos(meas.a[i]+mp.a);
					coord.y = mp.y + meas.r[i]*sin(meas.a[i]+mp.a);
					pix = omcl.O->toPixel(coord);
					if(pix.x>=0 && pix.x<Nx&&pix.y>=0&&pix.y<Ny)
						setRGBpix(rgb,pix.x,pix.y, Nx, 0,0, 255);
				}
				
				fprintf(stderr,"3: Gpos (%.4f) a=%.3f",fms, virt.a[0]);
				displayImage(0, rgb,Ny,Nx);
				
				
				cnt++;
		}
		
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		//G.save("labra.map");
		return 0;
}